#include "foc.h"
#include "svpwm.h"
#include "math/fast_math.h"
#include "bsp/bsp.h"

void foc_init(foc_t *foc) {
	pi_reset(&foc->pi_curr_id, 0);
	pi_reset(&foc->pi_curr_iq, 0);
	foc->e_velocity_filted = 0;
	foc->curr_dq_filted.d = 0;
	foc->curr_dq_filted.q = 0;
	foc->curr_dq_set.d = 0;
	foc->curr_dq_set.q = 0;
	foc->vol_dq_set.d = 0;
	foc->vol_dq_set.q = 0;
}

void foc_get_svm_duty(foc_t *foc) {
	
	float s = foc->phase_sin;
	float c = foc->phase_cos;
	float max_mag = foc->vbus_vol * (ONE_BY_SQRT3 * CONFIG_SVM_MODULATION);
	if (!foc->b_openloop){
		/* limiter Vd output for PI controller */
		float max_vd = max_mag;
		foc->pi_curr_id.max = max_vd;
		foc->pi_curr_id.min = -max_vd;
		float err = foc->curr_dq_set.d - foc->curr_dq_real.d;
		foc->vol_dq_out.d = pi_run(&foc->pi_curr_id, err, 0);
		/* limiter Vq output for PI controller */
		float max_vq = sqrtsub2_f(max_vd, foc->vol_dq_out.d);
		foc->pi_curr_iq.max = max_vq;
		foc->pi_curr_iq.min = -max_vq;
		err = foc->curr_dq_set.q - foc->curr_dq_real.q;
		foc->vol_dq_out.q = pi_run(&foc->pi_curr_iq, err, 0);
	}else {
		float max_vd = max_mag;
		foc->vol_dq_out.d = clamp_f(foc->vol_dq_set.d, -max_vd, max_vd);
		float max_vq = sqrtsub2_f(max_vd, foc->vol_dq_out.d);
		foc->vol_dq_out.q = clamp_f(foc->vol_dq_set.q, -max_vq, max_vq);
	}

	park_inv(foc->vol_dq_out.d, foc->vol_dq_out.q, s, c, &foc->vol_albt_out.a, &foc->vol_albt_out.b);
	float alpha_mod = foc->vol_albt_out.a / foc->vbus_vol;
	float beta_mod = foc->vol_albt_out.b / foc->vbus_vol;
	
	foc->sector = svpwm_get_duty(alpha_mod, beta_mod, foc->duty_norm);
}

